#include "../includes.h"

TRACE_	TRACE;

float TRACE_KP, TRACE_KI, TRACE_T;
extern int	gCenter, gMax;
int gTraceTableIndex;

#define TRACE_dDAC	10000
float PrevdDAC;

int Prev_DetailOffset;

int debug_trace=0;

void InitializeTraceControl();
void TraceControl();
void Init_TRACE_Control();

void CheckTraceControlDAC();


typedef struct{
	Matrics GainK, HatX, HatXm0, Pm0, P0, PhiK, QK, HK, IK, U;
	Matrics R;
	USER_ USER;
	double 	RK;
	float Kp, Ki, T;
	double CurrentU, PrevU;
}KF_; // kalman struct

KF_ KFtrace; //


void gnInit_KalmanVariable(KF_ *p)
{

	// 98-08-17 8:33
	//Kp = 0.05;
	//Ki = 0.05;
	//T = 0.1;

    // MATLAB SIMULATION FAST LOCK 98-11-22
    // GAIN SCHEDULING : INIT. LOCKING 0 ~ 50 step step!
	// [2000-01-20 14:53]
    p->Kp = 0.3;
	p->Ki = 0.05;
	p->T = 0.1;

	p->CurrentU=0;
	p->PrevU=0;

    //Kp = 0.1;
	//Ki = 0.025;
 	//T = 0.1;
	init_matrics(&p->Pm0,3,3,0);
	init_matrics(&p->P0,3,3,0);
	init_matrics(&p->PhiK,3,3,0);
	init_matrics(&p->QK,3,3,0);
	init_matrics(&p->GainK,3,1,0);
	init_matrics(&p->HatX,3,1,0);
	init_matrics(&p->HatXm0,3,1,0);
	init_matrics(&p->HK,1,3,0);
	init_matrics(&p->IK,3,3,0);
    init_matrics(&p->U,3,1,0);


    /*******************************/
    // 98-08-17 8:35
    // Init_KalmanVariable()
	//USER.R	 = 10;
	//USER.Q1   = 800;
	//USER.Q2   = 10;
	//USER.Q3   = 0.05;

    // MATLAB SIMULATION FAST LOCK 98-11-22
    // KInterval = Kinterval_STEP_3 INIT LOCKING 0 ~ 50 step step!
    p->USER.R = 10;
	p->USER.Q1 = 2000;
	p->USER.Q2 = 10;
	p->USER.Q3 = 0.02;

    p->USER.R = 10;
	p->USER.Q1 = 800;
	p->USER.Q2 = 10;
	p->USER.Q3 = 0.1;

	p->RK = USER.R;

	p->Pm0.mat[0][0] = 0;	p->Pm0.mat[0][1] = 0;	p->Pm0.mat[0][2] = 0;
	p->Pm0.mat[1][0] = 0;	p->Pm0.mat[1][1] = 0;	p->Pm0.mat[1][2] = 0;
	p->Pm0.mat[2][0] = 0;	p->Pm0.mat[2][1] = 0;	p->Pm0.mat[2][2] = 0;

    //PhiK.mat[0][0] = 0.36787944;	PhiK.mat[0][1] = 0;	PhiK.mat[0][2] = 0;
    // KInterval = Kinterval_STEP_4
    // MATLAB SIMULATION FAST LOCK 98-11-22
    // KInterval = Kinterval_STEP_3
    p->PhiK.mat[0][0] = 0.01831;        p->PhiK.mat[0][1] = 0;	p->PhiK.mat[0][2] = 0;
    p->PhiK.mat[1][0] = 0;		        p->PhiK.mat[1][1] = 1;	p->PhiK.mat[1][2] = 1;
    p->PhiK.mat[2][0] = 0;		        p->PhiK.mat[2][1] = 0;	p->PhiK.mat[2][2] = 1;

	p->QK.mat[0][0] = USER.Q1;	p->QK.mat[0][1] = 0;	    p->QK.mat[0][2] = 0;
	p->QK.mat[1][0] = 0;	    p->QK.mat[1][1] = USER.Q2;	p->QK.mat[1][2] = 0;
	p->QK.mat[2][0] = 0;	    p->QK.mat[2][1] = 0;	    p->QK.mat[2][2] = USER.Q3;

	p->IK.mat[0][0] = 1;	p->IK.mat[0][1] = 0;	    p->IK.mat[0][2] = 0;
	p->IK.mat[1][0] = 0;	p->IK.mat[1][1] = 1;       p->IK.mat[1][2] = 0;
	p->IK.mat[2][0] = 0;	p->IK.mat[2][1] = 0;       p->IK.mat[2][2] = 1;

	p->GainK.mat[0][0] = 0;
	p->GainK.mat[1][0] = 0;
	p->GainK.mat[2][0] = 0;

	p->HatX.mat[0][0] = 0;
	p->HatX.mat[1][0] = 0;
	p->HatX.mat[2][0] = 0;

	p->HatXm0.mat[0][0] = 0;
	p->HatXm0.mat[1][0] = 0;
	p->HatXm0.mat[2][0] = 0;

	p->HK.mat[0][0] = 1;	
	p->HK.mat[0][1] = 1;	
	p->HK.mat[0][2] = 0;
}


double KFPushAndGet(KF_ *p,double value )
{
		p->GainK = multiplyN(multiply(p->Pm0,transpose(p->HK)),(1/(MatToFloat(multiply(multiply(p->HK,p->Pm0),transpose(p->HK)))+p->RK)));
		p->HatX = Madd( p->HatXm0,(multiplyN( p->GainK,(value-MatToFloat(multiply( p->HK, p->HatXm0))))));
		p->P0 = multiply(Madd(  p->IK, multiplyN(multiply( p->GainK, p->HK),-1.0)), p->Pm0);

		p->U.mat[1][0] = p->PrevU;

		p->HatXm0 = Madd(multiply(p->PhiK,p->HatX),p->U);
		p->Pm0 = Madd( multiply(multiply(p->PhiK,p->P0),transpose(p->PhiK)),p->QK);

		p->PrevU = p->CurrentU;
		return p->HatXm0.mat[1][0];
}

void Init_TRACE_value()
{
	int i;
	gnInit_KalmanVariable(&KFtrace);
	TRACE_KP = 0.2;
	TRACE_KI = 0.05;
	TRACE_T = 0.05;
	//TRACE.NominalDAC
 	TRACE.MonitorTableIndex = 0;
 	for( i = 0; i < TraceMonitorTable_SIZE; i++ ){
 		TRACE.MonitorTable[i] = 0;
 	}

    TRACE.DACTableIndex = 0;
    TRACE.UTableIndex = 0;

	gTraceTableIndex = 0;
	TRACE.ControlCounter=0;
	
}
void Init_TRACE_Control()
{
	int i;
	Init_TRACE_value();
	TRACE.NominalDAC = (double)CONFIG.DAC;

 	TRACE.MonitorTableIndex = 0;
 	for( i = 0; i < TraceMonitorTable_SIZE; i++ ){
 		TRACE.MonitorTable[i] = 0;
 	}

	TRACE.MeanDAC = 0;

    TRACE.MeanU = 0;
    for( i = 0; i < MAX_TraceDACTable_SIZE; i++) TRACE.DACTable[i] = 0;
    for( i = 0; i < MAX_TraceUTable_SIZE; i++) TRACE.UTable[i] = 0;


	TRACE.ReferenceADC = 110;

	TRACE.TURN = TRACE_FIRST;
	TRACE.EVENT = OFF;

	TRACE.polarity = UNDEFINED;

	TRACE.ResyncCounter = 0;

	TRACE.control_turn = TRACE_CONTROL_FIRST;
	
	TRACE.DelayCounter = 2;

}


void InitializeTraceControl()
{
   	Tracking_Interval_Counter = 0;
	TRACE.ControlCounter = 0;

   	InitOffsetTable(&TraceOffsetTable);

	TRACE.MeanDAC = MeanDAC = CONTROL.NominalDAC = CONTROL.RealDAC;

	TRACE.DACTableDelayCounter = 10;
	TRACE.DACTableIndex = 0;

	LOCKEDtoMASTER_SPEC_IN = FAIL;
	init_trace_end = NO;

	//EVENT.PLL = ON;
	TRACE.IntervalCounter = 0;
	FIRST_STANDBY = YES;
	TRACE.EVENT = ON;

	TRACE.DelayCounter = 10;

	Prev_DetailOffset = 0;
	TRACE.MonitorTableIndex = 0;
	
	MessageOut("\n\n======== Init Trace Control ========\n\n");
}


void InitializeTraceControlSecond()
{
   	Tracking_Interval_Counter = 0;
	TRACE.ControlCounter = 0;

   	InitOffsetTable(&TraceOffsetTable);

	TRACE.MeanDAC = MeanDAC = CONTROL.NominalDAC = CONTROL.RealDAC;

	TRACE.DACTableDelayCounter = 10;
	TRACE.DACTableIndex = 0;

	//LOCKEDtoMASTER_SPEC_IN = FAIL;
	//init_trace_end = NO;

	//EVENT.PLL = ON;
	TRACE.IntervalCounter = 0;
	FIRST_STANDBY = YES;

	TRACE.DelayCounter = 10;
	
	Prev_DetailOffset = 0;
	TRACE.MonitorTableIndex = 0;
	
	MessageOut("n\n\n======== Initialize TraceControl Second ========\n\n");
}



void Start_Trace_Control()
{

	TRACE.IntervalCounter++;
	TRACE.ResyncCounter = 0;

	switch(TRACE.TURN){

		// 75/100
		case TRACE_FIRST://1
						SYSTEM.OPERATION.DETAIL_MODE = 1;
						if(fabs(GetCounterTrace())>2){
							CONTROL.TRACE_SYNC = ON;
							MessageOut("Trace Sync.!!\r\n");
						}
						else{
                    		MessageOut("Skip Trace Sync.!!\r\n");
                    	}
						TRACE.TURN = TRACE_SECOND;
						break;
		// 25/100
		case TRACE_SECOND://2
						TRACE.TURN = TRACE_THIRD;
						break;
		case TRACE_THIRD ://3

                    	TRACE.TURN = TRACE_SIXTH;
						break;
		case TRACE_SIXTH://6
						SYSTEM.OPERATION.DETAIL_MODE = 0;
                    	TRACE.control_turn = TRACE_CONTROL_FIRST;

    					if( TRACE.IntervalCounter > 2 ){
							TRACE.IntervalCounter = 0;

							TRACE.TURN = TRACE_FIRST;
							TRACE.EVENT = OFF;
							CounterMonitorFlag = ON;
							ClearFail();

							EnableClockFailLatch();

							TRACE.control_turn = TRACE_CONTROL_FIRST;
						}

						break;

	}
}

float detailSense=0;

int spec_in;
int iTraceMatchCnt=0;
char First_iTraceMatchCnt = YES;
char Status_Change = YES; 

void funcReadTraceCounter()
{

	int TraceDACNo,TraceUNo;
	int i;
	double TraceDACsum,TraceUsum;


	int DetailOffset;
	//////////int tableIndex=0;
	float detail;

	//float dDAC,ddDAC;
	//int refresh_value;
	//kang DetailOffset = ReadTraceCounterFPGA();
	DetailOffset = GetCounterTrace();

	if( DetailOffset > 4999999 ){
		DetailOffset = (DetailOffset - 10000000);
	}
	
	detailSense = (GetFpgaValue(32)&0x7F)-32;
	TRACE.detailSense = detailSense;
	detailSense = min(32, detailSense);
	detailSense = max(0, detailSense);
	
	detail = -0.03125*((double)detailSense) + 0.5;
	detail = detail*100.;
	
	//TRACE.TimeOffset = (int)KFPushAndGet(&KFtrace, (double)(DetailOffset*100 + (int)detail) );
	TRACE.TimeOffset = (double)(DetailOffset*100 + (int)detail) ;
	
	TRACE.TimeOffset -= (int)(CONFIG.delay_value);

	TRACE.TimeDifference = TRACE.PrevTimeOffset - TRACE.TimeOffset;
	TRACE.PrevTimeOffset = TRACE.TimeOffset;


}


int warmup_detail_offset;
float warmup_detail;
float warmup_detailSense=0;
double warmup_toffset;
int warmup_trace_holdup=0;

void Warmup_Trace_Control(void)
{
int wait_time;

	if((CounterModule.EXIST == YES) && (CONTROL.SYNC == OFF) && 
		((SYSTEM.EB.STATUS != EB_STATUS_RUN) || (SYSTEM.EB.LOCKMODE != EB_LOCKED)))
	{
		
		warmup_trace_holdup++;
		
		if(SYSTEM.ID == ModuleA){wait_time = 4;}
		else{wait_time = 7;}
			
		if(warmup_trace_holdup > wait_time){
			warmup_detail_offset = GetCounterTrace();
	
			if( warmup_detail_offset > 4999999 ){
				warmup_detail_offset = (warmup_detail_offset - 10000000);
			}
		
			warmup_detailSense = (GetFpgaValue(32)&0x7F)-32;
			warmup_detailSense = min(32, warmup_detailSense);
			warmup_detailSense = max(0, warmup_detailSense);
			
			warmup_detail = -0.03125*((double)warmup_detailSense) + 0.5;
			warmup_detail = warmup_detail*100.;
		
			warmup_toffset = (double)(warmup_detail_offset*100 + (int)warmup_detail) ;
			warmup_toffset -= (int)(CONFIG.delay_value);
		
			//over 200ms
			if(((warmup_toffset < -200000000) || (warmup_toffset > 200000000)) && (!IsSigOn())){
				Clear1PPSWithU1PPS();
				LogItString1(LOG_CATA1_NORM, LOG_CATA2_NOTHING, "WarmUp re-sync,", (int)warmup_toffset);
				warmup_trace_holdup = 0;
			}
		}
	}
	else{
		warmup_trace_holdup = 0;
	}

}


unsigned int trace_10m_now;
unsigned int trace_10m_prev;
unsigned int trace_10m_ok;
unsigned int trace_10m_err;
void TraceControl(void)
{

	int TraceDACNo,TraceUNo;
	int i;
	double TraceDACsum,TraceUsum;


	int DetailOffset;
	int tableIndex=0;
	float detail;

	float dDAC,ddDAC;


	int refresh_value;

	
//--------------------------------------------
//--------------------------------------------
//--------------------------------------------
//////////Read Counter	
	//kang DetailOffset = ReadTraceCounterFPGA();
	DetailOffset = GetCounterTrace();
	trace_10m_now = GetCounterTraceAll();

	//if((trace_10m_now == trace_10m_prev) && (CounterModule.EXIST == YES)){
	if(((trace_10m_now == trace_10m_prev) && (CounterModule.EXIST == YES)) ||
		(CounterModule.EXIST != YES))
	{
		trace_10m_err++;
		trace_10m_ok=0;
	}
	else{
		trace_10m_ok++;
		trace_10m_err=0;
	}
	trace_10m_prev = trace_10m_now;
	
	if((trace_10m_err>60) && (CounterModule.EXIST == YES)){
		trace_10m_err = 100;
		
		if(DIAG.TRACE_10M_COUNTER != FAIL){
			sprintf(MESG,"Master 10M incoming, %d",trace_10m_now);
			LogItString(LOG_CATA1_NORM, LOG_CATA2_FAIL, MESG);
		}
		DIAG.TRACE_10M_COUNTER = FAIL;
	}
	else if(trace_10m_ok>10){
		trace_10m_ok = 100;
		
		if((DIAG.TRACE_10M_COUNTER != OK) && (CounterModule.EXIST == YES)){
			sprintf(MESG,"Master 10M incoming, %d",trace_10m_now);
			LogItString(LOG_CATA1_NORM, LOG_CATA2_OK, MESG);
		}
		DIAG.TRACE_10M_COUNTER = OK;
	}
			
	if( DetailOffset > 4999999 ){
		DetailOffset = (DetailOffset - 10000000);
	}

	detailSense = (GetFpgaValue(32)&0x7F)-32;
	TRACE.detailSense = detailSense;
	detailSense = min(32, detailSense);
	detailSense = max(0, detailSense);
	
	detail = -0.03125*((double)detailSense) + 0.5;
	detail = detail*100.;
	
	//TRACE.TimeOffset = (int)KFPushAndGet(&KFtrace, (double)(DetailOffset*100 + (int)detail) );
	if(DIAG.DUAL_STEP == TRY_NULL){
		TRACE.TimeOffset = (double)(DetailOffset*100 + (int)detail) ;
		TRACE.TimeOffset -= (int)(CONFIG.delay_value);
	}
	
	if(debug_trace){
		//debug_trace = 0;
		TRACE.TimeOffset = 1000000;
	}
	
	TRACE.TimeDifference = TRACE.PrevTimeOffset - TRACE.TimeOffset;
	TRACE.PrevTimeOffset = TRACE.TimeOffset;

//--------------------------------------------
//--------------------------------------------
//--------------------------------------------

	if((CounterModule.EXIST != YES) || (DIAG.DUAL_STEP != TRY_NULL)){
		if (SYSTEM.IO.CTRL_PORT.MODE == UDEBUG){
			printf(
	    		"%3d:%2d:%2d:%2d [trace] %3u>"
	    		" R:%7.2f M:%7.2f RO:%5.2f ARO:%5.2f RD:%5.2f T:%5.2f %08d [--]\n",
	    		CONTROL.StepCounter, SYSTEM.TIME.CUR.HOUR, SYSTEM.TIME.CUR.MINUTE, SYSTEM.TIME.CUR.SECOND,
				TRACE.MonitorTableIndex,
				(float)CONTROL.RealDAC, (float)TRACE.MeanDAC, 
				(float)((double)TRACE.TimeOffset/100), (float)(TRACE.AvgTimeOffset*TENMILLION),
				(float)((double)TRACE.TimeDifference/100),
				(float)TEMPERATURE.CURRENT, GC.ACCsecond
			);
	    }
		return;
	}
	
 	if(TRACE.DelayCounter <= 0) {

		if( DetailOffset >= 1000 || DetailOffset <= -1000) { // in case of detection counter jump
			LogIt(LOG_CATA1_NORM, LOG_CATA2_NOTHING, LOG_TRACE,DetailOffset);
			//if( AreYouAlive() ){
			if(CounterModule.EXIST == YES){
				FIRST_STANDBY = YES;
//printf(">TR-1:%f,%f\n",CONTROL.RealDAC,(float)CONFIG.DAC);
				CONTROL.RealDAC = (double)CONFIG.DAC;
				InitializeTraceControl();
			}
		}
	}

	//if(!AreYouAlive()){	return; }
	//if(CounterModule.EXIST != YES){return;}

//printf("[[TR]]");


	if( TRACE.DelayCounter > 0 ) {
		TRACE.DelayCounter--;
		PrevdDAC = 0;
		TRACE.PrevAvgTimeOffset = 0;
		TRACE.PrevU = 0;
	}
	else {

    	TRACE.ControlCounter++;

		if(1) {

	    	TRACE.MonitorTable[(TRACE.MonitorTableIndex++)%TraceMonitorTable_SIZE] = TRACE.TimeOffset;

			iTraceMatchCnt=0;
	    	if ( TRACE.MonitorTableIndex >= TraceMonitorTable_SIZE ) {
				spec_in = YES;
		    	for(i = 0 ; i < TraceMonitorTable_SIZE; i++){
		    		if ( !( fabs(TRACE.MonitorTable[i])< 10 ) ) spec_in = NO;
					else iTraceMatchCnt++;
	    		}

	    		if (spec_in == YES) {
	    			init_trace_end = OK;
				}
				//printf(">>>1: %5d , %5d\n",iTraceMatchCnt,TRACE.MonitorTableIndex);
				if( iTraceMatchCnt>(TraceMonitorTable_SIZE-5) && (LOCKEDtoMASTER_SPEC_IN == FAIL) ){
					//RECOVERY.flag = OFF;
					LOCKEDtoMASTER_SPEC_IN = OK;
					//printf(">>>2: %5d , %5d\n",iTraceMatchCnt,TRACE.MonitorTableIndex);
				}
				if( iTraceMatchCnt<(TraceMonitorTable_SIZE-10) && (LOCKEDtoMASTER_SPEC_IN == OK) ){
					LOCKEDtoMASTER_SPEC_IN = FAIL;
					//printf(">>>3: %5d , %5d\n",iTraceMatchCnt,TRACE.MonitorTableIndex);
				}
	    	} else {
		    	for(i = 0 ; i < TRACE.MonitorTableIndex; i++){
		    		if ( !( fabs(TRACE.MonitorTable[i])< 15 ) ) LOCKEDtoMASTER_SPEC_IN = FAIL;
					else iTraceMatchCnt++;
	    		}
	    		//printf(">>>4: %5d , %5d\n",iTraceMatchCnt,TRACE.MonitorTableIndex);
	    	}

	    	TRACE.AvgTimeOffset = (double)(( (double)TRACE.TimeOffset) /1E9 );
	    	TRACE.CurrentU = -(TRACE_KP*TRACE.AvgTimeOffset-(TRACE_KP-TRACE_KI*TRACE_T)*TRACE.PrevAvgTimeOffset-TRACE.PrevU);
			dDAC = TRACE.CurrentU/(KGainF*TRACKING_INTERVAL);
			ddDAC = dDAC-PrevdDAC;

			if ( ddDAC < -TRACE_dDAC ) dDAC = PrevdDAC-TRACE_dDAC;
			if ( ddDAC >  TRACE_dDAC ) dDAC = PrevdDAC+TRACE_dDAC;

	    	CONTROL.RealDAC = dDAC + TRACE.NominalDAC;
//printf(">TR-2: %7.2f, %7.2f,  %d, %.3E, %.3E, %.3E, %.3E\n",
//	CONTROL.RealDAC,TRACE.NominalDAC,
//	TRACE.TimeOffset, TRACE.AvgTimeOffset,
//	TRACE.PrevAvgTimeOffset, TRACE.CurrentU, TRACE.PrevU
//);
			PrevdDAC = dDAC;

			CheckTraceControlDAC();

			//SetRbFreq();

	    	CONTROL.PrevDAC = CONTROL.RealDAC;
	    	TRACE.PrevAvgTimeOffset = TRACE.AvgTimeOffset;  // Master tracking Time offset
			TRACE.PrevU = TRACE.CurrentU;

			TRACE.UTable[(TRACE.UTableIndex++)%MAX_TraceUTable_SIZE] = TRACE.CurrentU;
	    	TraceUNo = (MAX_TraceUTable_SIZE > TRACE.UTableIndex ) ? TRACE.UTableIndex : MAX_TraceUTable_SIZE ;
	    	for( i = 0, TraceUsum = 0; i < TraceUNo  ; i++ ){
	   	        TraceUsum += TRACE.UTable[i];
	    	}
	    	TRACE.MeanU = TraceUsum / (double)TraceUNo;


			if( TRACE.DACTableDelayCounter > 0 ){
				TRACE.DACTableDelayCounter--;
			}
			else{
		    	TRACE.DACTable[(TRACE.DACTableIndex++)%MAX_TraceDACTable_SIZE] = CONTROL.RealDAC;
	    		TraceDACNo = (MAX_TraceDACTable_SIZE > TRACE.DACTableIndex ) ? TRACE.DACTableIndex : MAX_TraceDACTable_SIZE ;
	    		for( i = 0, TraceDACsum = 0; i < TraceDACNo  ; i++ ){
		   	        TraceDACsum += TRACE.DACTable[i];
	    		}
	    		TRACE.MeanDAC = TraceDACsum / (double)TraceDACNo;
	    	}
		}
	}

    //if (SYSTEM.IO.CTRL_PORT.MODE == UDEBUG){
	//	printf("\nTraceControl()--->SpecIn:<%d,%d,%3d>DAC:%7.2f, detail:%08d, %d, %d, %d<=%d, %5.2f", 
	//			spec_in, LOCKEDtoMASTER_SPEC_IN, iTraceMatchCnt,
	//			(float)CONTROL.RealDAC, GetCounterTraceAll(), (int)detailSense, (int)detail, (int)DetailOffset, TRACE.TimeOffset, TEMPERATURE.CURRENT  );
	//}
	//
    //if (SYSTEM.IO.CTRL_PORT.MODE == UDEBUG){
	//	printf(			"\n>>"
	//					"%3d>"
	//					"%2d:%2d:%2d "
	//					"[%3d] %3u>",                       
    //		CONTROL.StepCounter, 
	//		SYSTEM.TIME.CUR.HOUR, SYSTEM.TIME.CUR.MINUTE, SYSTEM.TIME.CUR.SECOND,
	//		(int)TraceMonitorTable_SIZE, TRACE.MonitorTableIndex
	//		);
    //    
	//	printf(	" R:%7.2f M:%7.2f",(float)CONTROL.RealDAC, (float)TRACE.MeanDAC);		
	//	printf(	" RO:%5.2f RD:%5.2f H:%6ld ",(float)RAW_OFFSET, (float)RAW_DIFFERENCE, (int)SYSTEM.POSITION.CURRENT.HEI);		
    //
	//	printf(	"ACCs:%d "
	//			"<<Detail,%d, %d, %d, %7.2f>>"
	//			"[,%d,%d,]"
    //			,(int)GC.ACCsecond,   (int)detailSense, (int)detail, (int)TRACE.TimeOffset, 	(float)TRACE.AvgTimeOffset
	//			,(int)TRACE.DelayCounter, (int)CounterModule.delay
	//		);
	//		
	//	//kang---------------------------------------------------------
	//	printf("<<ACT Detail : %d, %f>>", (int)detail_cnt, g_detail);
	//	//kang---------------------------------------------------------
	//	
    //}
    
    if (SYSTEM.IO.CTRL_PORT.MODE == UDEBUG){
		//printf(
    	//	"%3d:%2d:%2d:%2d [%3d] %3u>"
    	//	" R:%7.2f M:%7.2f RO:%5.2f RD:%5.2f T:%5.2f %08d [%d]\n"
    	//	"     trace>> <%d,%d,%3d> [%d,%d] <%d, %d, %d, %d, %d>\n",
    	//	CONTROL.StepCounter, SYSTEM.TIME.CUR.HOUR, SYSTEM.TIME.CUR.MINUTE, SYSTEM.TIME.CUR.SECOND,
		//	(int)TraceMonitorTable_SIZE, TRACE.MonitorTableIndex,
		//	(float)CONTROL.RealDAC, (float)TRACE.MeanDAC, (float)RAW_OFFSET, (float)RAW_DIFFERENCE, 
		//	TEMPERATURE.CURRENT, GetCounterTraceAll(), IsSigOn(),
		//	spec_in, LOCKEDtoMASTER_SPEC_IN, iTraceMatchCnt,
		//	(int)TRACE.DelayCounter, (int)CounterModule.delay,
		//	(int)detailSense, (int)detail, (int)DetailOffset, (int)TRACE.TimeOffset, 
		//	(int)(TRACE.AvgTimeOffset*BILLION)
		//	);
		printf(
    		"%3d:%2d:%2d:%2d [trace] %3u>"
    		" R:%7.2f M:%7.2f RO:%5.2f ARO:%5.2f RD:%5.2f T:%5.2f %08d [%d]\n",
    		CONTROL.StepCounter, SYSTEM.TIME.CUR.HOUR, SYSTEM.TIME.CUR.MINUTE, SYSTEM.TIME.CUR.SECOND,
			TRACE.MonitorTableIndex,
			(float)CONTROL.RealDAC, (float)TRACE.MeanDAC, 
			(float)((double)TRACE.TimeOffset/100), (float)(TRACE.AvgTimeOffset*TENMILLION),
			(float)((double)TRACE.TimeDifference/100),
			(float)TEMPERATURE.CURRENT, GC.ACCsecond, LOCKEDtoMASTER_SPEC_IN
		);
    }

//kang---------------------------------------------------------------------------------------------------------------	
	if( spec_in==1 && LOCKEDtoMASTER_SPEC_IN==1 && iTraceMatchCnt==50 && Status_Change == YES ){
		Status_Change = NO;
		LogItString2(LOG_CATA1_NORM, LOG_CATA2_NOTHING, 
			"SpecIn:<1, 1, 50> : spec_in, LOCKEDtoMASTER_SPEC_IN :", spec_in , LOCKEDtoMASTER_SPEC_IN);
	}
//-------------------------------------------------------------------------------------------------------------------

	Prev_DetailOffset = DetailOffset;



	if( TRACE.ControlCounter > 20*60 ){
		if ( LOCKEDtoMASTER_SPEC_IN != OK ) {
			TRACE.ControlCounter = 0;
			FIRST_STANDBY = YES;
			TRACE.EVENT = ON;
		}
	}

	DIAG.dDAC = OK;

}

#define mabs(x)	((x)>0?(x):-(x))

void TraceControl_ExtRef(void)
{
	int TraceDACNo,TraceUNo;
	int i;
	double TraceDACsum,TraceUsum;

	int DetailOffset;
	int tableIndex=0;
	float detail;

	float dDAC,ddDAC;
	int refresh_value;
	

//--------------------------------------------
	//kang DetailOffset = ReadTraceCounterFPGA();
	DetailOffset = GetCounterTrace();
//--------------------------------------------

	if( DetailOffset > 4999999 ){
		DetailOffset = (DetailOffset - 10000000);
	}
	
	//TmpDetailValue = (-1/(double)n100s)*(detail_cnt) + 0.5;
	detailSense = (GetFpgaValue(32)&0x7F)-32;
	detailSense = min(32, detailSense);
	detailSense = max(0, detailSense);
	
	//detail = (-1.0/(double)32.)*((double)detailSense) + 0.5;
	detail = -0.03125*((double)detailSense) + 0.5;
	detail = detail*100.;
	
	
	if(OCXOID.step <= 3){
		TRACE.TimeOffset = (int)(DetailOffset*100 + (int)detail);	
	}
	else{
		TRACE.TimeOffset = (int)KFPushAndGet(&KFtrace, (double)(DetailOffset*100 + (int)detail) );
	}

    //kang CONFIG.delay_value = 12;
	//kang CounterModule.delay = 12;
	//kang TRACE.TimeOffset -= (int)(CounterModule.delay+CONFIG.delay_value);
	//TRACE.TimeOffset -= (int)(CONFIG.delay_value);

	TRACE.TimeDifference = TRACE.PrevTimeOffset - TRACE.TimeOffset;
	TRACE.PrevTimeOffset = TRACE.TimeOffset;
//	TRACE.AverageADCValue = TRACE.TimeOffset - TRACE.ReferenceADC;


	
	if( OCXOID.step == 0 ){
		OCXOID.step_cnt++;
		CONTROL.CurrentDAC = 25000;
		OCXOID.RD_1st = 0;
		printf("25000 : %d, %f, %f\n", OCXOID.step_cnt, (float)TRACE.TimeDifference, OCXOID.RD_1st);
		
		if(OCXOID.step_cnt >= 5){
			OCXOID.step_cnt = 0;
			OCXOID.step = 1;
		}
		return;
	}
	else if( OCXOID.step == 1 ){
		CONTROL.CurrentDAC = 25000;
		OCXOID.step_cnt++;
		OCXOID.RD_1st += (float)TRACE.TimeDifference;
		printf("25000 : %d, %f, %f\n", OCXOID.step_cnt, (float)TRACE.TimeDifference, OCXOID.RD_1st);
		  
		if(OCXOID.step_cnt >= 20){
			OCXOID.step_cnt = 0;
			OCXOID.RD_1st = OCXOID.RD_1st/20.0; 
			printf("\nOCXOID.RD_1st = %f\n", OCXOID.RD_1st);
					
			OCXOID.step = 2;
		}
		return;
	}
	else if( OCXOID.step == 2 ){
		OCXOID.step_cnt++;
		CONTROL.CurrentDAC = 35000;
		OCXOID.RD_2nd = 0;
		printf("35000 : %d, %f, %f\n", OCXOID.step_cnt, (float)TRACE.TimeDifference, OCXOID.RD_2nd);
		
		if(OCXOID.step_cnt >= 5){
			OCXOID.step_cnt = 0;
			OCXOID.step = 3;
		}
		return;
	}
	else if( OCXOID.step == 3 ){
		CONTROL.CurrentDAC = 35000;
		OCXOID.step_cnt++;
		OCXOID.RD_2nd += (float)TRACE.TimeDifference;
		printf("35000 : %d, %f, %f\n", OCXOID.step_cnt, (float)TRACE.TimeDifference, OCXOID.RD_2nd);
		 
		if(OCXOID.step_cnt >= 20){
			OCXOID.step_cnt = 0;
			OCXOID.RD_2nd = OCXOID.RD_2nd/20.0;  
			printf("\nOCXOID.RD_2nd = %f\n", OCXOID.RD_2nd);
			OCXOID.step = 4;
		}
		return;
	}	
	else if( OCXOID.step == 4 ){
		CONTROL.RealDAC  = 35000;
		TRACE.NominalDAC = 35000;
		
		InitializeTraceControl();

		//KGainF = 1.2e-11;
		KGainF = ( (OCXOID.RD_2nd - OCXOID.RD_1st)/10000 ) /1E9;
		KGainF = mabs( KGainF );	
			
		printf("\nKGainF = %e\n", KGainF);
		
		printf("\n$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$\n");
		OCXOID.step = 5;
	}

	
	
	//--------------------------------------------------------------------------------------------
	OCXOID.trace_cnt++;
	OCXOID.ro_sum += TRACE.TimeOffset;
	

	switch( OCXOID.trace_interval ){
		case 1:
			if( mabs(detail ) < 8 ){
				OCXOID.match_cnt++;
				//cnt++
				//sum++

				if( OCXOID.match_cnt > 500 ){
					OCXOID.match_cnt = 0;
					OCXOID.trace_interval = 20;
					OCXOID.trace_cnt = 0;
					
					TRACE.NominalDAC = (double)CONTROL.CurrentDAC;
					TRACE.PrevU = 0;
					OCXOID.ro_sum =0;
					
					TRACE_KP = 0.3;
					TRACE_KI = 0.1;
					TRACE_T = 0.05;
						
					//cal center
				}
			}
			else OCXOID.match_cnt = 0;
					
			break;
			
		/*
		case 5:
			if( mabs(detail ) < 5 ){	
				OCXOID.match_cnt++;
				if( OCXOID.match_cnt > 300 ){
					OCXOID.match_cnt = 0;
					OCXOID.trace_interval = 10;
					OCXOID.trace_cnt=0;
				}
			}
			else if( mabs(detail ) > 20 ){
				OCXOID.trace_interval = 1;
				OCXOID.trace_cnt=0;
			}
			else OCXOID.match_cnt = 0;
							
			break;
		
		*/
		case 20:
			if( mabs( detail ) > 100 ){
				OCXOID.trace_interval = 1;
				OCXOID.trace_cnt=0;
				OCXOID.ro_sum =0;
			}
							
			break;	
				
	}


	
	//--------------------------------------------------------------------------------------------
	
	
	

	//--------------------------------------
	OCXOID.detailSense = (int)detailSense;
	OCXOID.detail = (int)detail;
	OCXOID.DetailOffset = (int)DetailOffset;
    //---------------------------------------

	
	if( OCXOID.trace_cnt % OCXOID.trace_interval == 0 && OCXOID.trace_cnt!=0){
		
		OCXOID.trace_cnt = 0;
		OCXOID.aveoffset = OCXOID.ro_sum/(double)OCXOID.trace_interval ;
		OCXOID.ro_sum = 0;
	
    	TRACE.ControlCounter++;


		//TRACE.AvgTimeOffset = (double)(((double)TRACE.TimeOffset)/1E9);
		TRACE.AvgTimeOffset = (double)(((double)OCXOID.aveoffset)/1E9);
		
		

		TRACE.CurrentU = -(TRACE_KP*TRACE.AvgTimeOffset-(TRACE_KP-TRACE_KI*TRACE_T)*TRACE.PrevAvgTimeOffset-TRACE.PrevU);

		//kang dDAC = TRACE.CurrentU/(KGainF*TRACKING_INTERVAL);
		dDAC = TRACE.CurrentU/(KGainF*OCXOID.trace_interval);

		ddDAC = dDAC-PrevdDAC;

		/*
		if ( ddDAC < -TRACE_dDAC ) dDAC = PrevdDAC-TRACE_dDAC;
		if ( ddDAC >  TRACE_dDAC ) dDAC = PrevdDAC+TRACE_dDAC;
		*/
	
		CONTROL.RealDAC = dDAC + TRACE.NominalDAC;

		PrevdDAC = dDAC;
			
		//CheckTraceControlDAC();

		//SetRbFreq();

		CONTROL.PrevDAC = CONTROL.RealDAC;
		TRACE.PrevAvgTimeOffset = TRACE.AvgTimeOffset;  // Master tracking Time offset
		TRACE.PrevU = TRACE.CurrentU;
			
		if( CONTROL.RealDAC < 10 ) CONTROL.RealDAC = 10;
		if( CONTROL.RealDAC > 65525 ) CONTROL.RealDAC = 65525;


		CONTROL.CurrentDAC = CONTROL.RealDAC;
		//CONTROL.CurrentDAC = (int)(CONTROL.RealDAC + OCXO_ID.TDAC); //OCXO-ID ���
			
	}


    if( SYSTEM.IO.CTRL_PORT.MODE == UDEBUG ){
		printf(			"\n>>"
						"%3d>"
						"%2d:%2d:%2d "
						"[%3d] %3u>",
    		CONTROL.StepCounter, 
			SYSTEM.TIME.CUR.HOUR, SYSTEM.TIME.CUR.MINUTE, SYSTEM.TIME.CUR.SECOND,
			(int)TraceMonitorTable_SIZE, TRACE.MonitorTableIndex
			);

		printf(			" R:%7.2f M:%7.2f",
  
			(float)CONTROL.RealDAC, (float)TRACE.MeanDAC
			
 		);
	
		printf("\n<%3d>DAC:%7.2f,>>>>> detail:%08d, %d, %d, %d<=%d", 
				iTraceMatchCnt,
				(float)CONTROL.RealDAC, GetCounterTraceAll(), (int)detailSense, (int)detail, (int)DetailOffset, TRACE.TimeOffset );
		

		printf( "<<Detail,%d, %d, %d, %7.2e>> %7.2e, %7.2e"
			,(int)detailSense, (int)detail, (int)TRACE.TimeOffset, 	(float)TRACE.AvgTimeOffset, TRACE.CurrentU, KGainF
			);
    }

	Prev_DetailOffset = DetailOffset;

	DIAG.dDAC = OK;


	if( abs(DetailOffset) > 1000 ){
		Clear1PPSWithU1PPS();
	}

}



void CheckTraceControlDAC()
{

	//if ( LOCKEDtoMASTER_SPEC_IN != OK ) {
	//
	//	if( CONTROL.RealDAC < EFC_LOWER_LIMIT ) {	CONTROL.RealDAC = EFC_LOWER_LIMIT;}
	//	if( CONTROL.RealDAC > EFC_UPPER_LIMIT ) {	CONTROL.RealDAC = EFC_UPPER_LIMIT;}
    //
	//}
	//else {
	//	if( CONTROL.RealDAC < 5 ) {	CONTROL.RealDAC = 5;}
	//	if( CONTROL.RealDAC > 65530 ) {	CONTROL.RealDAC = 65530;}
	//}
	if( CONTROL.RealDAC < EFC_LOWER_LIMIT ) {	CONTROL.RealDAC = EFC_LOWER_LIMIT;}
	if( CONTROL.RealDAC > EFC_UPPER_LIMIT ) {	CONTROL.RealDAC = EFC_UPPER_LIMIT;}
	
	CONTROL.ControlDAC = CONTROL.RealDAC;

}


